/**
 * 
 * @author Nixxx
 * 
 *         In this Class the robot first cruises around searching for balls,
 *         avoiding light sources. The task for avoiding light sources has an
 *         higher priority than the task for finding balls, because otherwise
 *         the robot would mistake a light source for a ball. If the robot has
 *         found a ball and pushes it in front if him, the task for moving to a
 *         light source has now a higher priority. As long as the robot has a
 *         ball at his front, he will move towards the light source, and if he
 *         would loose the ball again the ball finder would be activated. The
 *         liberator method has the highest priority and gets valid, if the
 *         robot is stuck at a light source or elsewhere. Some parts of
 *         liberating behavior is also implemented in the wallChecker() method.
 */

public class BallLightPusher1 extends Subsumption {

	/**
	 * Flags for behavior decision
	 */
	private boolean m_lightAvoidFlag;
	private boolean m_lightSearchFlag;
	private boolean m_liberatorFlag;
	/**
	 * Motor speeds for each behavior
	 */
	private int[] m_lightAvoidOutput = { 0, 0 };
	private int[] m_lightSearchOutput = { 0, 0 };
	private int[] m_liberatorOutput = { 0, 0 };
	/**
	 * count for doing liberator move if robot is stuck a specific time
	 */
	private int m_liberatorCount;


	public BallLightPusher1() {
		setWaitTime(7);
		m_cruiserFlag = false;
		m_wcheckerFlag = false;
		m_bfinderFlag = false;
		m_bpusherFlag = false;
		m_lightAvoidFlag = false;
		m_lightSearchFlag = false;
		m_liberatorFlag = false;

		m_liberatorCount = 0;

		m_milage = 0;
	}


	public void arbitrate() {

		if (m_cruiserFlag == true) {
			System.out.println("Cruise");
			setMotorSpeeds(m_cruiseOutput[0], m_cruiseOutput[1]);
		}

		if (m_bfinderFlag == true) {
			System.out.println("Ballfinder");
			setMotorSpeeds(m_bfinderOutput[0], m_bfinderOutput[1]);
		}

		if (m_lightAvoidFlag == true) {
			System.out.println("LightAvoid");
			setMotorSpeeds(m_lightAvoidOutput[0], m_lightAvoidOutput[1]);
		}

		if (m_bpusherFlag == true) {
			System.out.println("Ballpusher");
			setMotorSpeeds(4, 4);
		}
		if (m_lightSearchFlag == true) {
			System.out.println("LightSearch");
			setMotorSpeeds(m_lightSearchOutput[0], m_lightSearchOutput[1]);
		}

		if (m_wcheckerFlag == true) {
			System.out.println("Avoid");
			setMotorSpeeds(m_wcheckerOutput[0], m_wcheckerOutput[1]);
			this.sleep(1200);
		}
		if (m_liberatorFlag == true) {
			System.out.println("liberate");
			setMotorSpeeds(m_liberatorOutput[0], m_liberatorOutput[1]);
			this.sleep(1200);
		}

	}


	public void doWork() throws Exception {
		liberator();

		if (m_liberatorFlag == false) {
			wallChecker();

			if (m_wcheckerFlag == false) {
				lightSearch();

				if (m_lightSearchFlag == false) {
					ballPusher();

					if (m_bpusherFlag == false) {
						lightAvoid();

						if (m_lightAvoidFlag == false) {
							ballFinder();

							if (m_bfinderFlag == false) {
								cruise();
							}

						}

					}

				}

			}

		}

		arbitrate();
		setMilage();
	}


	/**
	 * this method is for avoiding light sources, because they are not movable
	 * and robot would get stuck
	 */
	public void lightAvoid() {
		getLightValues();
		getSensorValues();

		if (l0 < LOW_LIGHTTHRESHOLD || l1 < LOW_LIGHTTHRESHOLD
				|| l2 < LOW_LIGHTTHRESHOLD) {
			m_lightAvoidOutput[0] = 4;
			m_lightAvoidOutput[1] = 2;
			m_lightAvoidFlag = true;
		} else if (l3 < LOW_LIGHTTHRESHOLD || l4 < LOW_LIGHTTHRESHOLD
				|| l5 < LOW_LIGHTTHRESHOLD) {
			m_lightAvoidOutput[0] = 2;
			m_lightAvoidOutput[1] = 4;
			m_lightAvoidFlag = true;
		} else {
			m_lightAvoidFlag = false;
		}
	}


	/**
	 * this method is for driving towards a light source if a ball is pushed
	 */
	public void lightSearch() {
		getLightValues();
		getSensorValues();

		if ((s2 >= SENSOR_MAX || s3 >= SENSOR_MAX)
				&& (l0 < MID_LIGHTTHRESHOLD || l1 < MID_LIGHTTHRESHOLD || l2 < MID_LIGHTTHRESHOLD)) {
			m_lightSearchOutput[0] = 1;
			m_lightSearchOutput[1] = 2;
			m_lightSearchFlag = true;
		} else if ((s2 >= SENSOR_MAX || s3 >= SENSOR_MAX)
				&& (l3 < MID_LIGHTTHRESHOLD || l4 < MID_LIGHTTHRESHOLD || l5 < MID_LIGHTTHRESHOLD)) {
			m_lightSearchOutput[0] = 2;
			m_lightSearchOutput[1] = 1;
			m_lightSearchFlag = true;
		} else {
			m_lightSearchFlag = false;
		}

	}


	/**
	 * this method is for moving backwards and liberate after the robot is stuck
	 * for 100 cycles at e.g. a light source .
	 */
	public void liberator() {
		if (m_milage == getActualMilage() && (m_liberatorCount > 100)) {
			m_liberatorOutput[0] = -6;
			m_liberatorOutput[1] = -1;
			m_liberatorFlag = true;
			m_liberatorCount = 0;
		} else if (m_milage == getActualMilage()) {
			m_liberatorFlag = false;
			m_liberatorCount++;
		} else {
			m_liberatorFlag = false;
		}
	}
}
